#!/usr/bin/env python

"""
    ROS Sensors Class for the cmRobot Element
    
    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2012 Patrick Goebel.  All rights reserved.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.
    
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:
    
    http://www.gnu.org/licenses/gpl.html
"""

import roslib; roslib.load_manifest('element')
import rospy
from element.msg import SensorState
import os
import threading
from sensor import Sensor

""" Class to poll sensors attached to the Element """
class Sensors():
    def __init__(self, controller, thread_queue):
        self.controller = controller
                
        self.sensors = list()

        try:
            self.analog_sensors = rospy.get_param("~analog", dict({}))
            for name, params in self.analog_sensors.iteritems():
                self.sensors.append(Sensor(self.controller, name, digital=False, type=params['type'], rate=params['rate'], pin=params['pin']))
                rospy.loginfo(name + " " + str(params))
        except:
            pass
        
        try:
            self.digital_sensors = rospy.get_param("~digital", dict({}))
            for name, params in self.digital_sensors.iteritems():
                self.sensors.append(Sensor(self.controller, name, digital=True, type=params['type'], rate=params['rate'], pin=params['pin']))
                rospy.loginfo(name + " " + str(params))
        except:
            pass
                
        try:
            self.maxez1_sensors = rospy.get_param("~maxez1", dict({}))
            for name, params in self.maxez1_sensors.iteritems():
                self.sensors.append(Sensor(self.controller, name, digital=True, type=params['type'], rate=params['rate'], pin=params['pin'], trigger_pin=params['trigger_pin'], output_pin=params['output_pin']))
                rospy.loginfo(name + " " + str(params))
        except:
            pass
        
    def poll_sensors(self, thread_queue, sensor):
        t = threading.Timer(sensor.interval, self.poll_sensors, [thread_queue, sensor])
        t.daemon = True
        t.start()
        thread_queue.put(sensor.poll)
                
if __name__ == '__main__':
    try:
        sensors = Sensors()
    except:
        pass
